#include "periph_devices.hpp"

#include "support-periph-devices/devices/mcu_device.hpp"
#include "support-periph-devices/devices/driver_device.hpp"
#include "support-periph-devices/devices/rfid_device.hpp"
#include "support-periph-devices/devices/encoder_device.hpp"
#include "support-periph-devices/support_proto.hpp"
#include "support-mqtt/mqtt.hpp"

#include "devices/detail/define.hpp"

#include <log/log.hpp>
#include <commons/sync_async/async_timer.hpp>

PeriphDevices::PeriphDevices(const std::string &name, const std::string &module)
    : Node(name, module), mcu_device_(nullptr), mqtt_(nullptr), pub_timer_(nullptr)
{
}

RetBool PeriphDevices::init()
{
    std::string root = util::env_var("HOME");
    if (root.empty())
    {
        root = ".";
    }

    // 日志初始化
    std::string log_path = util::format("{}/.sros/{}/{}/log", root, info().module, info().name);
    std::string log_filename = util::format("{}.log", info().name);
    Log::Param param;
    param.path = log_path;
    param.file_name = log_filename;
    Log::ins().init(param);

    // mqtt 客户端初始化
    plugins::MqttParam mqtt_pram;
    mqtt_pram.id = info().module + "_" + info().name;
    mqtt_pram.keepalive = 10;
    mqtt_pram.on_error = [](const int &&ec, const std::string &&msg)
    {
        sros_err << "ec " << ec << " msg " << msg << sros_endl; 
    };
    mqtt_pram.on_status_changed = [](const int &&s1, const std::string &&m1, const int &&s2, const std::string &&m2)
    {
        sros_info << m1 << " " << m2 << sros_endl;
    };
    mqtt_pram.remote_ip = "0.0.0.0";
    mqtt_pram.remote_port = 1883;
    mqtt_pram.reconnect_delay = 1;
    mqtt_pram.reconnect_delay_max = 10;
    mqtt_pram.reconnect_exponential_backoff = true;
    mqtt_ = std::make_shared<sros::plugins::Mqtt>(mqtt_pram);

    // 外设初始化
    mcu_device_ = std::make_shared<device::McuDevice>();
    driver_device_ = std::make_shared<device::DriverDevice>(mcu_device_);
    rfid_device_ = std::make_shared<device::RfidDevice>(mcu_device_);
    encoder_device_ = std::make_shared<device::EncoderDevice>(mcu_device_);

    auto ret = mcu_device_->init();
    if (!ret.v)
    {
        return ret;
    }

    ret = driver_device_->init();
    if (!ret.v)
    {
        return ret;
    }

    ret = rfid_device_->init();
    if (!ret.v)
    {
        return ret;
    }

    ret = encoder_device_->init();
    if (!ret.v)
    {
        return ret;
    }

    pub_timer_ = std::make_shared<async::Timer>();

    mcu_device_->install_can_status_cb([this](const device::proto::CanStatus &sts)
    {
        std::string cont = util::format("CAN{} active:{}, error:{}, state:{}, msg:{}", 
            sts.index, (int)sts.active, sts.error, (int)sts.state, sts.msg);
        LOG_INFO(cont);
    });

    mcu_device_->install_uart_status_cb([this](const device::proto::UartStatus &sts)
    {
        std::string cont = util::format("UART{} active:{}, error:{}, state:{}, msg:{}", 
            sts.index, (int)sts.active, sts.error, (int)sts.state, sts.msg);
        LOG_INFO(cont);
    });

    mcu_device_->install_can_msg_cb([this](const device::proto::RspCanMsg &can_msg)
    {
        std::string cont = util::format("[CAN{}] [{}] [{}] [{}]", 
            can_msg.index, 
            util::hex_to_string(can_msg.msg.Header.StdId), 
            can_msg.msg.Header.DLC, 
            util::hex_to_string(can_msg.msg.Data));
        LOG_INFO(cont);

        driver_device_->parse(can_msg);
    });

    mcu_device_->install_uart_msg_cb([this](const device::proto::RspUartMsg &uart_msg)
    {
        std::string cont = util::format("[UART{}] [{}] [{}]", 
            uart_msg.index, 
            uart_msg.buffer.size(), 
            util::hex_to_string(uart_msg.buffer));
        LOG_INFO(cont);

        rfid_device_->parse(uart_msg);
    });

    mcu_device_->install_power_status_cb([this](const std::map<uint32_t, bool> &power_status)
    {
        std::string cont = "[Power]";
        for (auto &it : power_status)
        {
            auto tmp = util::format(" [{},{}]", it.first, it.second);
            cont += tmp;
        }
        LOG_INFO(cont);
    });

    mcu_device_->install_encoder_msg_cb([this](const std::map<uint32_t, int32_t> &encoder_msg)
    {
        std::string cont = "[Encoder]";
        for (auto &it : encoder_msg)
        {
            auto tmp = util::format(" [{},{}]", it.first, it.second);
            cont += tmp;
        }
        LOG_INFO(cont);

        encoder_device_->parse(encoder_msg);
    });

    driver_device_->install_event([this](const uint8_t &ev_id, const uint8_t &dir, const uint32_t &data, const std::string &msg)
    {
        PubDriverEvent event = {.ev_id = ev_id, .dir = dir, .ev_data = data, .ev_msg = msg};
        nlohmann::json j = event;
        std::string cont = j.dump();
        std::vector<uint8_t> payload(cont.begin(), cont.end());

        mqtt_->pub("/sros/device/driver/event", payload);
    });

    rfid_device_->install_event([this](const uint8_t &ev_id, const uint32_t &data, const std::string &msg)
    {
        PubRfidEvent event { .ev_id = ev_id, .ev_data = data, .ev_msg = msg };
        nlohmann::json j = event;
        std::string cont = j.dump();
        std::vector<uint8_t> payload(cont.begin(), cont.end());

        mqtt_->pub("/sros/device/rfid/event", payload);
    });

    mqtt_->sub("/sros/device/driver/cmd", [this](const std::vector<uint8_t> &&payload)
    {
        try
        {
            auto cmd = nlohmann::json::parse(payload).get<DriverCmd>();

            driver_device_->set_spd(cmd.dir, cmd.data);
        }
        catch (const std::exception &e)
        {
            LOG_ERROR("[MQTT] parse /sros/device/driver/cmd failed, {}", e.what());
        }
    });

    mqtt_->sub("test", [this](const std::vector<uint8_t> &&payload)
    {
        auto j = nlohmann::json::parse(payload);
        int cmd = j["cmd"];
        std::cout << "cmd :"  << cmd << std::endl;
        std::vector<uint8_t> pdo_data(8, 0);
        pdo_data[0] = cmd;
        driver_device_->set_control(cmd);
    });

    return ret;
}

void PeriphDevices::run()
{
    if (mqtt_)
    {
        mqtt_->run();
    }

    if (mcu_device_)
    {
        mcu_device_->run();
    }

    if (driver_device_)
    {
        driver_device_->run();
    }

    if (rfid_device_)
    {
        rfid_device_->run();
    }

    if (encoder_device_)
    {
        encoder_device_->run();
    }

    pub_timer_->safe_start(1000, 50, 0, [this](std::size_t times)
    {
        if (driver_device_)
        {
            device::detail::driver_impl::DataUp h_data_up;
            device::detail::driver_impl::DataUp v_data_up;
            device::detail::driver_impl::DataUp p_data_up;
            device::detail::driver_impl::DataUp r_data_up;

            driver_device_->data_up(device::cfg::DriverDir::H, h_data_up);
            driver_device_->data_up(device::cfg::DriverDir::V, v_data_up);
            driver_device_->data_up(device::cfg::DriverDir::P, p_data_up);
            driver_device_->data_up(device::cfg::DriverDir::R, r_data_up);

            device::detail::driver_impl::DataDown h_data_down;
            device::detail::driver_impl::DataDown v_data_down;
            device::detail::driver_impl::DataDown p_data_down;
            device::detail::driver_impl::DataDown r_data_down;

            driver_device_->data_down(device::cfg::DriverDir::H, h_data_down);
            driver_device_->data_down(device::cfg::DriverDir::V, v_data_down);
            driver_device_->data_down(device::cfg::DriverDir::P, p_data_down);
            driver_device_->data_down(device::cfg::DriverDir::R, r_data_down);

            device::detail::Info h_info;
            device::detail::Info v_info;
            device::detail::Info p_info;
            device::detail::Info r_info;

            driver_device_->info(device::cfg::DriverDir::H, h_info);
            driver_device_->info(device::cfg::DriverDir::V, v_info);
            driver_device_->info(device::cfg::DriverDir::P, p_info);
            driver_device_->info(device::cfg::DriverDir::R, r_info);

            device::detail::driver_impl::Param h_param;
            device::detail::driver_impl::Param v_param;
            device::detail::driver_impl::Param p_param;
            device::detail::driver_impl::Param r_param;
        
            driver_device_->param(device::cfg::DriverDir::H, h_param);
            driver_device_->param(device::cfg::DriverDir::V, v_param);
            driver_device_->param(device::cfg::DriverDir::P, p_param);
            driver_device_->param(device::cfg::DriverDir::R, r_param);

            // 把内部数据合并成需要对外发布的数据
            auto info_to_driver_info = [](const device::detail::driver_impl::Param &param,
                                          const device::detail::Info &info,
                                          DriverInfo &driver_info)
            {
                driver_info.id = param.id;
                driver_info.hardware_version = info.hardware_version;
                driver_info.software_version = info.software_version;
                driver_info.datetime = info.datetime;
                driver_info.manufacturer = info.manufacturer;
                driver_info.model = info.model;
            };

            // 把内部数据合并成需要对外发布的数据
            auto data_up_to_driver_status = [](const device::detail::driver_impl::DataUp &data_up, 
                                               const device::detail::driver_impl::Param  &param,
                                               const device::detail::Info &info, 
                                               DriverStatus &driver_status)
            {
                driver_status.id = param.id;
                driver_status.current = data_up.current;
                driver_status.temperature = data_up.temperature;
                driver_status.speed = data_up.spd;
                driver_status.rpm = data_up.rpm;
                driver_status.pos = data_up.pos;
                driver_status.pulse = data_up.pulse;
                driver_status.error = data_up.error;
                driver_status.status_word = data_up.status_word;
                driver_status.state = data_up.state;
                driver_status.mode = data_up.mode;
                driver_status.enable = data_up.enable;
            };

            // 发布数据电机状态数据 50ms
            PubDriverStatus pub_driver_status;

            DriverStatus driver_status;
            data_up_to_driver_status(h_data_up, h_param, h_info, driver_status);
            pub_driver_status.H.push_back(driver_status);

            data_up_to_driver_status(v_data_up, v_param, v_info, driver_status);
            pub_driver_status.V = driver_status;

            data_up_to_driver_status(p_data_up, p_param, p_info, driver_status);
            pub_driver_status.P = driver_status;

            data_up_to_driver_status(r_data_up, r_param, r_info, driver_status);
            pub_driver_status.R = driver_status;

            nlohmann::json j = pub_driver_status;
            std::string cont = j.dump();
            std::vector<uint8_t> payload(cont.begin(), cont.end());
            auto ret = mqtt_->pub("/sros/device/driver/status", payload);
            if (!ret.v)
            {
                LOG_WARN("pub /sros/device/driver/status failed, {}", ret.msg);
            }

            j = nlohmann::json::array();
            // 发布数据编码器状态数据 50ms
            auto encoder_data = encoder_device_->data();
            for (const auto &it : encoder_data)
            {
                EncoderStatus status;
                status.id = it.first;
                status.pulse = it.second.pulse;
                status.pos = it.second.pos;
                j.push_back(status);
            }
            cont = j.dump();
            payload = std::vector<uint8_t>(cont.begin(), cont.end());
            ret = mqtt_->pub("/sros/device/enc_ab/status", payload);
            if (!ret.v)
            {
                LOG_WARN("pub /sros/device/enc_ab/status failed, {}", ret.msg);
            }

            j = nlohmann::json();

            // 每1000ms 发布一次电机信息数据
            if (times % 20 == 0)
            {
                PubDriverInfo pub_driver_info;
                DriverInfo driver_info;

                info_to_driver_info(h_param, h_info, driver_info);
                pub_driver_info.H.push_back(driver_info);

                info_to_driver_info(v_param, v_info, driver_info);
                pub_driver_info.V = driver_info;

                info_to_driver_info(p_param, p_info, driver_info);
                pub_driver_info.P = driver_info;

                info_to_driver_info(r_param, r_info, driver_info);
                pub_driver_info.R = driver_info;

                j = pub_driver_info;
                cont = j.dump();
                std::vector<uint8_t> payload(cont.begin(), cont.end());

                ret = mqtt_->pub("/sros/device/driver/info", payload);
                if (!ret.v)
                {
                    LOG_WARN("pub /sros/device/driver/info failed, {}", ret.msg);
                }
            }
        }
    });
}

void PeriphDevices::close()
{
    if (pub_timer_)
    {
        pub_timer_->safe_stop();
        pub_timer_->safe_clear();
    }

    if (encoder_device_)
    {
        encoder_device_->close();
    }

    if (rfid_device_)
    {
        rfid_device_->close();
    }

    if (driver_device_)
    {
        driver_device_->close();
    }

    if (mcu_device_)
    {
        mcu_device_->close();
    }

    if (mqtt_)
    {
        mqtt_->close();
    }

    Log::ins().deinit();
}
